/*
 * File:   sensors.c
 *
 * Created on April 8, 2013, 2:41 PM
 */

#include <p24FJ64GA002.h>
#include "sensors.h"

#define OBRIEN_CONSTANT 400

volatile unsigned int previousSensorValues[4][10];
volatile int currentFront = 0;

void InitializeSensors(void) {
    int i, j;

    /* Sets AN0-1 & AN4 and AN9 to analog */
    /* AN0 -> IR Sensor
     * AN1 -> Middle Phototransistor
     * AN4 -> Left Phototransistor
     * AN9 -> Right Phototransistor
     */
    AD1PCFG = 0x1DEC;
    TRISAbits.TRISA0 = 1;

    /* Configure A/D module for IR and phototransitor sensors */
    AD1CON1 = 0x20E6;
    AD1CON2 = 0x40C;
    AD1CON3 = 0x213;
    AD1CSSL = 0x213;

    // initialize value arrays
    for (i = 0; i < 4; i++) {
        for (j = 0; j < 10; j++) {
            previousSensorValues[i][j] = 0;
        }
    }
}

void StartSensors(void) {
    /* Enable interrupts for analog pins */
    AD1CON1bits.ADON = 1;
    _AD1IF = 0;
    _AD1IE = 1;
}

void PauseSensors(void) {
    /* Enable interrupts for analog pins */
    AD1CON1bits.ADON = 0;
    _AD1IF = 0;
    _AD1IE = 0;
}

/* ADC ISR */
void __attribute__((interrupt, auto_psv)) _ADC1Interrupt(void) {
    _AD1IF = 0;
    // insert into current buffer position
    int newFront = (currentFront + 1) % 10;
    previousSensorValues[SENSOR_IR][newFront] = ADC1BUF0;
    previousSensorValues[SENSOR_MIDDLE_PHOTO][newFront] = ADC1BUF1;
    previousSensorValues[SENSOR_LEFT_PHOTO][newFront] = ADC1BUF3;
    previousSensorValues[SENSOR_RIGHT_PHOTO][newFront] = ADC1BUF2;
    currentFront = newFront;
    }

unsigned int GetCurrentSensorValue(int sensor) {
    return previousSensorValues[sensor][currentFront];
}

#define NUM_COEF 3

unsigned int GetFilteredSensorValue(int sensor) {
    int count;

    static const float kCoef[NUM_COEF] =
        {0.6f, 0.3f, 0.1f};
    
    unsigned int val = 0;
    int pos = currentFront;
    for (count = 0; count < NUM_COEF; count++) {
        val += (unsigned int)(((float)previousSensorValues[sensor][pos]) *
                kCoef[count]);
        pos = (pos + 1) % 10;
    }
    return val;
}

int IsMiddlePhotoOnTape()
{
    if (GetFilteredSensorValue(SENSOR_MIDDLE_PHOTO) > OBRIEN_CONSTANT)
        return 1;
    else
        return 0;
}

int IsRightPhotoOnTape()
{
    if (GetFilteredSensorValue(SENSOR_RIGHT_PHOTO) > OBRIEN_CONSTANT)
        return 1;
    else
        return 0;
}

int IsLeftPhotoOnTape()
{
    if (GetFilteredSensorValue(SENSOR_LEFT_PHOTO) > OBRIEN_CONSTANT)
        return 1;
    else
        return 0;
}
